package kinect_biomec.Logic;

import java.text.DecimalFormat;


import Testes.SimpleOpenNI;

import java.util.logging.Level;
import java.util.logging.Logger;
import processing.core.*;

public class KinectView extends PApplet {

    public SimpleOpenNI context;
    CalculoCentroDeGravidade calc;
    private boolean showingScene = false;
    private boolean showJustUser = false;
    private boolean showingRGB = false;
    private boolean showingDepth = true;
    private boolean needUpdate = false;
    private int skelProfile;
    private boolean calcPontoDeMass3D = false;
    private boolean calcPontoDeMass2D = false;
    private boolean calcPontoDeMassMembros = false;

    public String msg;
    

    public void echo(String msg){
        this.msg=msg;
    }
    
    @Override
    public void setup() {
        // context = new SimpleOpenNI(this, SimpleOpenNI.RUN_MODE_MULTI_THREADED);
        // context.enableDepth();
        background(200, 0, 0);
        stroke(0, 0, 255);
        strokeWeight(3);
        smooth();
        size(640, 480);
        
        // context.startTrackingSkeleton(SimpleOpenNI.SKEL_PROFILE_ALL);
        // calc = new CalculoCentroDeGravidade(this.context);
    }

    public void setContext(SimpleOpenNI context) {
        this.context = context;
        
        calc = new CalculoCentroDeGravidade(this.context);
    }

    public void clean() {
        if (needUpdate) {
            image(new PImage(640 * 2, 480), 0, 0);
            needUpdate = false;
        }
    }
    int[] imagePixels;
    PImage image;
    int[] userPixels;
    int i = 0;

    public void drawJustUsers() {
    }

    @Override
    public void draw() {
        // update the cam

        clean();

        if (context != null) {
            try {
                context.update();

                if (showingScene) {
                    image(context.sceneImage(), 0, 0);
                    if (showingRGB) {
                        image(context.rgbImage(), 640, 0);
                    }
                } else if (showingDepth) {
                    image(context.depthImage(), 0, 0);
                    if (showingRGB) {
                        image(context.rgbImage(), 640, 0);
                    }
                } else if (showingRGB) {
                    image(context.rgbImage(), 0, 0);
                }

                text("FPS: " + frameRate, 20, 20);
                fill(0, 255, 255);


                if (context.isTrackingSkeleton(1) && skelProfile!=SimpleOpenNI.SKEL_PROFILE_NONE) {
                    drawSkeleton(1);
                }


            } catch (Exception e) {
                e.printStackTrace();
                try {
                    synchronized (this) {
                        wait(2000);
                    }
                } catch (InterruptedException ex) {
                    Logger.getLogger(KinectView.class.getName()).log(Level.SEVERE, null, ex);
                }

            }
        }
    }
    PVector peEsquerdo;

    void drawPe(int userId) {
        peEsquerdo = new PVector();

        context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_FOOT,
                peEsquerdo);
        peEsquerdo.mult(1.1f);
        peEsquerdo.rotate(1.1f);
        ellipse(peEsquerdo.x, peEsquerdo.y, 15, 15);
    }
    PVector eixoQuadril;
    PVector neck;

    void drawTronco(int userId) {
        eixoQuadril = calc.getEixoDoQuadril(userId);
        neck = new PVector();

        context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_NECK, neck);

        neck = calc.converterPara2D(neck);

        line(neck.x, neck.y, eixoQuadril.x, eixoQuadril.y);
    }
    PVector centro;

    void drawCentroDeGravidadeTronco(int userId) {
        centro = calc.getCentroGravidadeDoTronco(userId);
        ellipse(centro.x, centro.y, 5, 5);
    }
    PVector[] centroDoBraco;

    void drawCentroGravidadeDaParteSuperiorDoBraco(int userId) {
        centroDoBraco = calc.getCentroGravidadeDaParteSuperiorDosBracos(userId);

        ellipse(centroDoBraco[CalculoCentroDeGravidade.esquerdo].x, centroDoBraco[CalculoCentroDeGravidade.esquerdo].y,
                8, 8);
        ellipse(centroDoBraco[CalculoCentroDeGravidade.direito].x, centroDoBraco[CalculoCentroDeGravidade.direito].y,
                8, 8);
    }
    PVector[] centroDaCoxa;

    void drawCentroGravidadeDaCoxa(int userId) {
        centroDaCoxa = calc.getCentroGravidadeDasCoxas(userId);

        ellipse(centroDaCoxa[CalculoCentroDeGravidade.esquerdo].x, centroDaCoxa[CalculoCentroDeGravidade.esquerdo].y,
                8, 8);
        ellipse(centroDaCoxa[CalculoCentroDeGravidade.direito].x, centroDaCoxa[CalculoCentroDeGravidade.direito].y, 8,
                8);
    }
    PVector[] centroDaPanturrilha;

    void drawCentroGravidadeDaPanturrilha(int userId) {
        centroDaPanturrilha = calc.getCentroGravidadeDaPanturrilha(userId);

        ellipse(centroDaPanturrilha[CalculoCentroDeGravidade.esquerdo].x,
                centroDaPanturrilha[CalculoCentroDeGravidade.esquerdo].y, 8, 8);
        ellipse(centroDaPanturrilha[CalculoCentroDeGravidade.direito].x,
                centroDaPanturrilha[CalculoCentroDeGravidade.direito].y, 8, 8);
    }
    PVector[] centroDoAntebraco;

    void drawCentroGravidadeDoAntebraco(int userId) {
        centroDoAntebraco = calc.getCentroGravidadeDoAntebraco(userId);
        ellipse(centroDoAntebraco[CalculoCentroDeGravidade.esquerdo].x,
                centroDoAntebraco[CalculoCentroDeGravidade.esquerdo].y, 8, 8);
        ellipse(centroDoAntebraco[CalculoCentroDeGravidade.direito].x,
                centroDoAntebraco[CalculoCentroDeGravidade.direito].y, 8, 8);
    }

    public void calcAndDrawPointOfMass(int userId) {
        if (calcPontoDeMass2D) {
            centro = calc.getCentroDeGravidade2(userId);
             ellipse(centro.x, centro.y, 10, 10);
        }
        if(calcPontoDeMassMembros){
            drawCentroDeGravidadeTronco(userId);
            drawCentroGravidadeDaCoxa(userId);
            drawCentroGravidadeDaParteSuperiorDoBraco(userId);
            drawCentroGravidadeDaPanturrilha(userId);
            drawCentroGravidadeDoAntebraco(userId);
        }
        if(calcPontoDeMass3D){
            drawPointOfMass(userId);
            
        }
        
    }

    public void drawSkeleton(int userId) {

        
        calcAndDrawPointOfMass(userId);

        

        if(skelProfile==SimpleOpenNI.SKEL_PROFILE_ALL ||
                skelProfile==SimpleOpenNI.SKEL_PROFILE_HEAD_HANDS || 
                skelProfile==SimpleOpenNI.SKEL_PROFILE_UPPER){
            
            context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);
            
            context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER,
                    SimpleOpenNI.SKEL_LEFT_ELBOW);

            context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,
                    SimpleOpenNI.SKEL_LEFT_HAND);
        

            context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER,
                    SimpleOpenNI.SKEL_RIGHT_ELBOW);

            context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,
                    SimpleOpenNI.SKEL_RIGHT_HAND);
            context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);
            
            if(skelProfile!=SimpleOpenNI.SKEL_PROFILE_HEAD_HANDS){
                drawTronco(userId);
            }
        }
        
        if(skelProfile==SimpleOpenNI.SKEL_PROFILE_ALL ||
                skelProfile==SimpleOpenNI.SKEL_PROFILE_LOWER){
            
            context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP,
                    SimpleOpenNI.SKEL_LEFT_KNEE);

            context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE,
                    SimpleOpenNI.SKEL_LEFT_FOOT);

            context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP,
                    SimpleOpenNI.SKEL_RIGHT_KNEE);

            context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE,
                    SimpleOpenNI.SKEL_RIGHT_FOOT);

            drawPe(userId);

        }


        //PVector max = calc.converterPara2D(calc.getPontoMaisAlto(userId));

        // System.out.println(max.y);
        //PVector min = calc.converterPara2D(calc.getPontoMaisBaixo(userId));

        //ellipse(min.x, max.y + 5, 10, 10);
        //ellipse(min.x, min.y + 15, 10, 10);

        // line(min.x,max.y,min.x,min.y);
        // float angulo = PVector.angleBetween(calc.getPontoMaisAlto(userId),
        // calc.getPontoMaisBaixo(userId));
        // float distancia = (float) calc.getDistancia(userId);

        // float altura = angulo;

        /*
         * text("Altura: " + altura + "\nDistancia (kinect-pontoM): " +
         * df.format(distancia) +
         * "\n Diferença entre os centros de gravidade:\n" + "	X: " +
         * df.format(Math.abs(getPointOfMass(userId).x /
         * calc.getCentroDeGravidade(userId).x)) + "%\n" + "	Y: " +
         * df.format(Math.abs(getPointOfMass(userId).y /
         * calc.getCentroDeGravidade(userId).y)) + "%", 10, 10); fill(0, 255,
         * 0);
         */
        // System.out.println(df.format(altura));

    }
    DecimalFormat df = new DecimalFormat("#.##");
    PVector pontoM;
    PVector pontoMNormalizado;



    public void drawPointOfMass(int userId) {

        pontoMNormalizado = calc.converterPara2D(calc.getCentroDeGravidade(userId));
        ellipse(pontoMNormalizado.x, pontoMNormalizado.y, 10, 10);
        // line(pontoMNormalizado.x, pontoMNormalizado.y,0,pontoMNormalizado.y);
        // line(pontoMNormalizado.x,
        // pontoMNormalizado.y,pontoMNormalizado.x,480);
    }

    // -----------------------------------------------------------------
    // SimpleOpenNI events
    // -----------------------------------------------------------------
    // SimpleOpenNI events
    public void onNewUser(int userId) {
        echo("Novo usuario - userId: " + userId+" iniciando detecção de pose");

        context.startPoseDetection("Psi", userId);
    }

    public void onLostUser(int userId) {
        echo("Usuario perdido - userId: " + userId);
    }

    public void onStartCalibration(int userId) {
        echo("Iniciando calibração - userId: " + userId);
    }

    public void onEndCalibration(int userId, boolean successfull) {
        echo("Calibração finalizada- userId: " + userId + ", status: " + successfull);

        if (successfull) {
            echo("  Usuario calibrado !!!");
            context.startTrackingSkeleton(userId);
        } else {
            echo("  Falha ao calibrar usuario !!!");
            //println("  Start pose detection");
            context.startPoseDetection("Psi", userId);
        }
    }

    public void onStartPose(String pose, int userId) {
        echo("Iniciando pose de calibração - userId: " + userId + ", pose: " + pose);
        //println(" stop pose detection");

        context.stopPoseDetection(userId);
        context.requestCalibrationSkeleton(userId, true);

    }

    public void onEndPose(String pose, int userId) {
        echo("Usuario deixou pose - userId: " + userId + ", pose: " + pose);
    }

    public boolean isNeedUpdate() {
        return needUpdate;
    }

    public void setNeedUpdate(boolean needUpdate) {
        this.needUpdate = needUpdate;
    }

    public boolean isShowJustUser() {
        return showJustUser;
    }

    public void setShowJustUser(boolean showJustUser) {
        this.showJustUser = showJustUser;
    }

    public boolean isShowingDepth() {
        return showingDepth;
    }

    public void setShowingDepth(boolean showingDepth) {
        this.showingDepth = showingDepth;
    }

    public boolean isShowingRGB() {
        return showingRGB;
    }

    public void setShowingRGB(boolean showingRGB) {
        this.showingRGB = showingRGB;
    }

    public boolean isShowingScene() {
        return showingScene;
    }

    public void setShowingScene(boolean showingScene) {
        this.showingScene = showingScene;
    }

    public int getSkelProfile() {
        return skelProfile;
    }

    public void setSkelProfile(int skelProfile) {
        this.skelProfile = skelProfile;
    }

    public boolean isCalcPontoDeMass2D() {
        return calcPontoDeMass2D;
    }

    public void setCalcPontoDeMass2D(boolean calcPontoDeMass2D) {
        this.calcPontoDeMass2D = calcPontoDeMass2D;
    }

    public boolean isCalcPontoDeMass3D() {
        return calcPontoDeMass3D;
    }

    public void setCalcPontoDeMass3D(boolean calcPontoDeMass3D) {
        this.calcPontoDeMass3D = calcPontoDeMass3D;
    }

    public boolean isCalcPontoDeMassMembros() {
        return calcPontoDeMassMembros;
    }

    public void setCalcPontoDeMassMembros(boolean calcPontoDeMassMembros) {
        this.calcPontoDeMassMembros = calcPontoDeMassMembros;
    }


}